#ifndef DSO_PIXEL_SELECTOR_H
#define DSO_PIXEL_SELECTOR_H

#include "dso/util/NumType.h"
#include "dso/util/Settings.h"

namespace dso
{
const float minUseGrad_pixsel = 10;

enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3};

class FrameHessian;

// 像素选取器
class PixelSelector
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    PixelSelector ( int w, int h );
    ~PixelSelector();

    // input:
    // fh-帧
    // map_out-点标记
    // density-需要的点数量
    int makeMaps (
        const FrameHessian* const fh,
        float* map_out, float density, int recursionsLeft=1, bool plot=false, float thFactor=1 );

    /**
     * @brief 生成图像直方图特征
     * @param frame
     */
    void makeHists ( const FrameHessian* const fh );

    int currentPotential =3;
    bool allowFast=false;

private:
    // 内部select函数
    Eigen::Vector3i select ( const FrameHessian* const fh,
                             float* map_out, int pot, float thFactor=1 );

    unsigned char* randomPattern;
    int* gradHist =nullptr;
    float* ths;
    float* thsSmoothed;
    int thsStep;
    const FrameHessian* gradHistFrame;
};



template<int pot>
inline int gridMaxSelection ( Eigen::Vector3f* grads, bool* map_out, int w, int h, float THFac )
{

    memset ( map_out, 0, sizeof ( bool ) *w*h );

    int numGood = 0;
    for ( int y=1; y<h-pot; y+=pot ) {
        for ( int x=1; x<w-pot; x+=pot ) {
            int bestXXID = -1;
            int bestYYID = -1;
            int bestXYID = -1;
            int bestYXID = -1;

            float bestXX=0, bestYY=0, bestXY=0, bestYX=0;

            Eigen::Vector3f* grads0 = grads+x+y*w;
            for ( int dx=0; dx<pot; dx++ )
                for ( int dy=0; dy<pot; dy++ ) {
                    int idx = dx+dy*w;
                    Eigen::Vector3f g=grads0[idx];
                    float sqgd = g.tail<2>().squaredNorm();
                    float TH = THFac*minUseGrad_pixsel * ( 0.75f );

                    if ( sqgd > TH*TH ) {
                        float agx = fabs ( ( float ) g[1] );
                        if ( agx > bestXX ) {
                            bestXX=agx;
                            bestXXID=idx;
                        }

                        float agy = fabs ( ( float ) g[2] );
                        if ( agy > bestYY ) {
                            bestYY=agy;
                            bestYYID=idx;
                        }

                        float gxpy = fabs ( ( float ) ( g[1]-g[2] ) );
                        if ( gxpy > bestXY ) {
                            bestXY=gxpy;
                            bestXYID=idx;
                        }

                        float gxmy = fabs ( ( float ) ( g[1]+g[2] ) );
                        if ( gxmy > bestYX ) {
                            bestYX=gxmy;
                            bestYXID=idx;
                        }
                    }
                }

            bool* map0 = map_out+x+y*w;

            if ( bestXXID>=0 ) {
                if ( !map0[bestXXID] ) {
                    numGood++;
                }
                map0[bestXXID] = true;

            }
            if ( bestYYID>=0 ) {
                if ( !map0[bestYYID] ) {
                    numGood++;
                }
                map0[bestYYID] = true;

            }
            if ( bestXYID>=0 ) {
                if ( !map0[bestXYID] ) {
                    numGood++;
                }
                map0[bestXYID] = true;

            }
            if ( bestYXID>=0 ) {
                if ( !map0[bestYXID] ) {
                    numGood++;
                }
                map0[bestYXID] = true;

            }
        }
    }

    return numGood;
}


inline int gridMaxSelection ( Eigen::Vector3f* grads, bool* map_out, int w, int h, int pot, float THFac )
{

    memset ( map_out, 0, sizeof ( bool ) *w*h );

    int numGood = 0;
    for ( int y=1; y<h-pot; y+=pot ) {
        for ( int x=1; x<w-pot; x+=pot ) {
            int bestXXID = -1;
            int bestYYID = -1;
            int bestXYID = -1;
            int bestYXID = -1;

            float bestXX=0, bestYY=0, bestXY=0, bestYX=0;

            Eigen::Vector3f* grads0 = grads+x+y*w;
            for ( int dx=0; dx<pot; dx++ )
                for ( int dy=0; dy<pot; dy++ ) {
                    int idx = dx+dy*w;
                    Eigen::Vector3f g=grads0[idx];
                    float sqgd = g.tail<2>().squaredNorm();
                    float TH = THFac*minUseGrad_pixsel * ( 0.75f );

                    if ( sqgd > TH*TH ) {
                        float agx = fabs ( ( float ) g[1] );
                        if ( agx > bestXX ) {
                            bestXX=agx;
                            bestXXID=idx;
                        }

                        float agy = fabs ( ( float ) g[2] );
                        if ( agy > bestYY ) {
                            bestYY=agy;
                            bestYYID=idx;
                        }

                        float gxpy = fabs ( ( float ) ( g[1]-g[2] ) );
                        if ( gxpy > bestXY ) {
                            bestXY=gxpy;
                            bestXYID=idx;
                        }

                        float gxmy = fabs ( ( float ) ( g[1]+g[2] ) );
                        if ( gxmy > bestYX ) {
                            bestYX=gxmy;
                            bestYXID=idx;
                        }
                    }
                }

            bool* map0 = map_out+x+y*w;

            if ( bestXXID>=0 ) {
                if ( !map0[bestXXID] ) {
                    numGood++;
                }
                map0[bestXXID] = true;

            }
            if ( bestYYID>=0 ) {
                if ( !map0[bestYYID] ) {
                    numGood++;
                }
                map0[bestYYID] = true;

            }
            if ( bestXYID>=0 ) {
                if ( !map0[bestXYID] ) {
                    numGood++;
                }
                map0[bestXYID] = true;

            }
            if ( bestYXID>=0 ) {
                if ( !map0[bestYXID] ) {
                    numGood++;
                }
                map0[bestYXID] = true;

            }
        }
    }

    return numGood;
}


inline int makePixelStatus ( Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1 )
{
    if ( sparsityFactor < 1 ) {
        sparsityFactor = 1;
    }

    int numGoodPoints;


    if ( sparsityFactor==1 ) {
        numGoodPoints = gridMaxSelection<1> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==2 ) {
        numGoodPoints = gridMaxSelection<2> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==3 ) {
        numGoodPoints = gridMaxSelection<3> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==4 ) {
        numGoodPoints = gridMaxSelection<4> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==5 ) {
        numGoodPoints = gridMaxSelection<5> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==6 ) {
        numGoodPoints = gridMaxSelection<6> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==7 ) {
        numGoodPoints = gridMaxSelection<7> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==8 ) {
        numGoodPoints = gridMaxSelection<8> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==9 ) {
        numGoodPoints = gridMaxSelection<9> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==10 ) {
        numGoodPoints = gridMaxSelection<10> ( grads, map, w, h, THFac );
    } else if ( sparsityFactor==11 ) {
        numGoodPoints = gridMaxSelection<11> ( grads, map, w, h, THFac );
    } else {
        numGoodPoints = gridMaxSelection ( grads, map, w, h, sparsityFactor, THFac );
    }


    /*
     * #points is approximately proportional to sparsityFactor^2.
     */

    float quotia = numGoodPoints / ( float ) ( desiredDensity );

    int newSparsity = ( sparsityFactor * sqrtf ( quotia ) ) +0.7f;


    if ( newSparsity < 1 ) {
        newSparsity=1;
    }


    float oldTHFac = THFac;
    if ( newSparsity==1 && sparsityFactor==1 ) {
        THFac = 0.5;
    }


    if ( ( abs ( newSparsity-sparsityFactor ) < 1 && THFac==oldTHFac ) ||
            ( quotia > 0.8 &&  1.0f / quotia > 0.8 ) ||
            recsLeft == 0 ) {

//              printf(" \n");
        //all good
        sparsityFactor = newSparsity;
        return numGoodPoints;
    } else {
//              printf(" -> re-evaluate! \n");
        // re-evaluate.
        sparsityFactor = newSparsity;
        return makePixelStatus ( grads, map, w,h, desiredDensity, recsLeft-1, THFac );
    }
}



}

#endif
